function [xr, dr] = reset_map(x, d)

n  = fix(length(x)/2);
M  = De_mat(x);
J  = E_mat(x);

dx = [0; 0; x(n+1:end)];
P  = [M, -J'; J, zeros(size(J, 1))] \ [M * dx; zeros(size(J, 1), 1)];
R1 = [1  1  1 -1 -1;
      0  0  0  0  1;
      0  0  0  1  0;
      0  0  1  0  0;
      0  1  0  0  0];
        
xr = [R1 * x(1:n); R1*P(3:n+2)];

jp = jointpos(x);
p_nsf = jp(1, end);

dr = d + p_nsf;

end
